#include "mpu6050_ALL.h"
//#include "GUI.h"
//#include "Lcd_Driver.h"
//#include "inv_mpu.h"

#include <stdio.h>
#include <math.h>

struct _MPU_Offset MPU_1_Offset={8.5,-7.1},MPU_2_Offset={6.7,-17.4};

struct _MPU_Single MPU_Single;
struct _MPU_Value MPU_1_Value,MPU_2_Value;
struct _MPU_dmp_value MPU_dmp_value;

int MPU_ALL_Init(void)
{
	if(MPU_Init() != 0)
		return -20;
	if(MPU_2_Init() != 0)
		return -25;
	
	return 1;
}


int MPU_ALL_Update(void)
{
//	MPU_1_Value.temp=MPU_Get_Temperature();	//ֵ
	MPU_Get_Accelerometer(&MPU_1_Value.aacx,&MPU_1_Value.aacy,&MPU_1_Value.aacz);	//�õ����ٶȴ���������
	MPU_Get_Gyroscope(&MPU_1_Value.gyrox,&MPU_1_Value.gyroy,&MPU_1_Value.gyroz);	//�õ�����������
	MPU_1_Value.pitch = (float)(((atan2(MPU_1_Value.aacy,MPU_1_Value.aacz)*180)/3.14159265));
	MPU_1_Value.roll = (float)(((atan2(MPU_1_Value.aacx,MPU_1_Value.aacz)*180)/3.14159265));
	
//	MPU_2_Value.temp=MPU_2_Get_Temperature();	//ֵ
	MPU_2_Get_Accelerometer(&MPU_2_Value.aacx,&MPU_2_Value.aacy,&MPU_2_Value.aacz);	//�õ����ٶȴ���������
	MPU_2_Get_Gyroscope(&MPU_2_Value.gyrox,&MPU_2_Value.gyroy,&MPU_2_Value.gyroz);	//�õ�����������
	MPU_2_Value.pitch = (float)(((atan2(MPU_2_Value.aacy,MPU_2_Value.aacz)*180)/3.14159265));
	MPU_2_Value.roll = (float)(((atan2(MPU_2_Value.aacx,MPU_2_Value.aacz)*180)/3.14159265));
	
	MPU_Single.prime = MPU_1_Value.pitch-MPU_1_Offset.prime;
	MPU_Single.vice = MPU_2_Value.pitch-MPU_2_Offset.prime;
	
	return 1;
}

//int MPU_DMP_Update(void)
//{
//	if(mpu_dmp_get_data(&MPU_dmp_value.pitch,&MPU_dmp_value.roll,&MPU_dmp_value.yaw)==0)
//	{
//		return 1;
//	}
//	return -1;
//}
